#include "main.h"

PID sPID;

void PIDInit(PID *pp, double P, double I, double D, double SetPoint)
{
	memset(pp, 0, sizeof(PID));
	sPID.P = P;
	sPID.I = I;
	sPID.D = D;
	sPID.SetPoint = SetPoint;
}

double PIDCalc(PID *pp, double NextPoint)
{
	double result;
	double dError, Error;
	Error = pp->SetPoint - NextPoint;
	pp->SumError += Error;
	dError = Error - pp->LastError;
	pp->LastError = Error;
	result = pp->P*Error + pp->I*pp->SumError+pp->D*dError;
	return result;
}